#include "rclcpp/rclcpp.hpp"
#include "rclcpp_action/rclcpp_action.hpp"
#include "ros2_cpp_pkg/action/move_robot.hpp"
#include "example_interfaces/msg/empty.hpp"


typedef ros2_cpp_pkg::action::MoveRobot MoveRobot;
typedef rclcpp_action::ClientGoalHandle<MoveRobot> MoveRobotGoalHandle;
using MoveRobotClient = rclcpp_action::Client<MoveRobot>;
using Empty = example_interfaces::msg::Empty;
// using std::placeholders::_1;
// using std::placeholders::_2;
using namespace std::placeholders;



class MoveRobotActionClientNode : public rclcpp::Node
{
public:
  MoveRobotActionClientNode(): Node("move_robot_client_node")
  {
    move_robot_client_ = rclcpp_action::create_client<MoveRobot>(this, 
    "move_robot");

    cancel_move_subscriber_ = this->create_subscription<Empty>(
      "move_robot_cancel", 10,
      std::bind(&MoveRobotActionClientNode::cancel_move_callback, this, _1));

    RCLCPP_INFO(this->get_logger(), "Action client has been started");
  }

  void send_goal(int position, int velocity)
  {
    move_robot_client_->wait_for_action_server();

    auto goal = MoveRobot::Goal();
    goal.position = position;
    goal.velocity = velocity;

    auto options = MoveRobotClient::SendGoalOptions();
    options.goal_response_callback = std::bind(
      &MoveRobotActionClientNode::goal_response_callback, this, _1);
    options.feedback_callback = std::bind(
      &MoveRobotActionClientNode::goal_feedback_callback, this, _1, _2);
    options.result_callback = std::bind(
      &MoveRobotActionClientNode::goal_result_callback, this, _1);

    RCLCPP_INFO(this->get_logger(), 
      "Sending goal: position=%d, velocity=%d", position, velocity);
    move_robot_client_->async_send_goal(goal, options);
  }

private:
  void goal_response_callback(const MoveRobotGoalHandle::SharedPtr &goal_handle)
  {
    if (!goal_handle)
    {
      RCLCPP_ERROR(this->get_logger(), "Goal was rejected by server");
    }
    else
    {
      this->goal_handle_ = goal_handle;
      RCLCPP_INFO(this->get_logger(), "Goal was accepted by server, waiting for result");
    }
  }

  void goal_feedback_callback(
    const MoveRobotGoalHandle::SharedPtr &goal_handle,
    const std::shared_ptr<const MoveRobot::Feedback> feedback)
  {
    (void)goal_handle;
    int position = feedback->current_position;
    RCLCPP_INFO(this->get_logger(), "Feedback position: %d", position);
  }

  void goal_result_callback(const MoveRobotGoalHandle::WrappedResult &result)
  {      
    switch (result.code)
    {
    case rclcpp_action::ResultCode::SUCCEEDED:
      RCLCPP_INFO(this->get_logger(), "Goal succeeded");
      break;
    case rclcpp_action::ResultCode::ABORTED:
      RCLCPP_WARN(this->get_logger(), "Goal aborted");
      break;
    case rclcpp_action::ResultCode::CANCELED:
      RCLCPP_WARN(this->get_logger(), "Goal canceled");
      break;
    default:
      RCLCPP_ERROR(this->get_logger(), "Unknown goal state");
    }
    int position = result.result->position;
    std::string message = result.result->message;
    RCLCPP_INFO(this->get_logger(), "Result: ");
    RCLCPP_INFO(this->get_logger(), " position: %d", position);
    RCLCPP_INFO(this->get_logger(), " message: %s", message.c_str());
  }

  void cancel_move_callback(const Empty::SharedPtr msg)
  {
    (void)msg;
    cancel_goal();
    
  }

  void cancel_goal()
  {
    if (this->goal_handle_)
    {
      RCLCPP_INFO(this->get_logger(), "Cancel request sent");
      this->move_robot_client_->async_cancel_goal(goal_handle_);
      goal_handle_.reset();
    }
  }

  rclcpp_action::Client<MoveRobot>::SharedPtr move_robot_client_;
  rclcpp::Subscription<Empty>::SharedPtr cancel_move_subscriber_;
  MoveRobotGoalHandle::SharedPtr goal_handle_;
};


int main(int argc, char *argv[])
{
  rclcpp::init(argc, argv);
  auto move_robot_client_node = std::make_shared<MoveRobotActionClientNode>();

  move_robot_client_node->send_goal(70, 1);
  rclcpp::executors::MultiThreadedExecutor executer;
  executer.add_node(move_robot_client_node);
  executer.spin();
  // rclcpp::spin(node);
  rclcpp::shutdown();
  return 0;
}